#include "ms_misc.h"

/* **************************************************************************** */
double VectorNorm( const double p[], const int n )
{
	double v = 0.;
	int i;
	for( i=0; i<n; v+=p[i]*p[i++] );;
	return sqrt(v);
}
/* **************************************************************************** */
double Distance( const double p[], const double q[] )
{
	return (sqrt( (p[0]-q[0])*(p[0]-q[0]) + (p[1]-q[1])*(p[1]-q[1]) + (p[2]-q[2])*(p[2]-q[2]) ) );
}
/* **************************************************************************** */
double VectorDotProduct( const double p[], const double q[], const int n )
{
	double v=0.;
	int i;
	for( i=0; i<n; v+=p[i]*q[i++] );;
	return v;
}
/* **************************************************************************** */
double PointDistance( const double p[],	const double q[])
{
	return sqrt(		(p[0]-q[0])*(p[0]-q[0])
					+	(p[1]-q[1])*(p[1]-q[1])
					+	(p[2]-q[2])*(p[2]-q[2])	);
}
/* **************************************************************************** */
void GetRotationMatrix(		double Q[3][3], const double N[] )
{
	const double  beta = N[2];
	if( fabs(beta) > 0.99 ) {
		Q[0][1] = Q[0][2] = Q[1][0] = Q[1][2] = Q[2][0] = Q[2][1] = 0.;
		Q[0][0] = Q[1][1] = Q[2][2] = 1.;
		if(beta<0.) Q[2][2] = -1.;
	}
	else {
		
		const double	gamma	= sqrt( N[0]*N[0] + N[1]*N[1] ),
						n1 = N[0] / gamma, n2= N[1] / gamma,
						n[] = { n1, n2 }, 
						phi		= atan2(gamma, beta),
						cphi	= cos(phi),
						sphi	= sin(phi);

						
		Q[2][0] = Q[2][1] = Q[0][2] = Q[1][2] = 0.; Q[2][2] = cphi;
		
		for(int i=0;i<2;i++) 
		{
			for(int j=0;j<2;j++)
				Q[i][j]=n[i]*n[j]*(1.-cphi);
			Q[i][i]+=cphi;
		}
		
		Q[0][2] += sphi*n2;    Q[1][2] -= sphi*n1;
		Q[2][0] -= sphi*n2;    Q[2][1] += sphi*n1;
	}
}
/* **************************************************************************** */
/* **************************************************************************** */
/* **************************************************************************** */
/* **************************************************************************** */
